#pragma once
#include <Reconstruct3D/SensorResource.h>
#include <Reconstruct3D/ProcessBase.hpp>
#include "CheckBoard.h"


struct PatternCalibrationData
{
	PatternCalibrationData (CheckBoard& i_checkBoard) : scale (1.0f),
			checkBoard (i_checkBoard),
			cornersAcc (i_checkBoard.getRows () * i_checkBoard.getCols ()),
			zAccArr (i_checkBoard.getRows () * i_checkBoard.getCols ()),
			accArr (i_checkBoard.getRows () * i_checkBoard.getCols ()){}
	bool hasInvaildZ () {
		for (int i = 0; i < cornersAcc.size (); ++i)
			if (zAccArr[i] == 0)
				return true;
		return false;
	}
	CheckBoard& checkBoard;
	float scale;
	std::vector< cv::Point2f > cornersAcc;
	std::vector< int > zAccArr;
	std::vector< int > accArr;
};

class PatternCalibrationProcess : public ProcessBase
{
public:
	PatternCalibrationProcess (const ReconstructConfig& config,
			int checkBoardRows, int checkBoardCols, float checkBlockSize);
	~PatternCalibrationProcess ();

	void initialize (SensorResource &sensorRes);
	void clean ();

	void process ();

private:
	CheckBoard _checkBoard;
	std::vector< glm::vec3 > _cornersInCamSpace;
	SensorResource* _rp_sensorRes;
	PatternCalibrationData _data;

	bool _recognizePattern (const QImage& image);
	void _calculateCornersInCameraSpace ();
	glm::mat4 _calculateExtrinsicMatrix ();
	glm::mat4 _calculateTranslation ();
	glm::mat4 _calculateRotation ();
	glm::mat4 _calculateScale ();
	void _getCameraMatrix ();
	void _showResult (const QImage& image, glm::mat4x4 rtsmat);
	glm::mat4 _calculateExtrinsicMatrixCv ();
	glm::mat4 _calculateExtrinsicMatrixBySolveEquation ();
};
